#include <C8051F340.H>
#include <intrins.h>
#include "config.h"

volatile unsigned long speed_ticks_curr = 0x00;
volatile unsigned long speed_ticks_last_interval = 0x00;
volatile unsigned char speed_is_set = 0x00;
volatile unsigned long speed_memory = 0x00;

//function prototypes
void PCA_Init();
void Timer_Init();
void Port_IO_Init();
void Oscillator_Init();
void Init_Device();

static void check_resume();
static void check_set();
static void check_velocity();

static void idle();


void PCA_Init()
{
    PCA0MD    &= ~0x40;
    PCA0MD    = 0x00;
}


void Timer_Init()
{
    TCON      = 0x15;
    TMOD      = 0x11;
    CKCON     = 0x02;
    TL0       = 0xBD;
    TH0       = 0x0B;
}


void Oscillator_Init()
{
    OSCICN    = 0x83;
}

void Port_IO_Init()
{
    // P0.0  -  Unassigned,  Push-Pull,  Digital
    // P0.1  -  Unassigned,  Push-Pull,  Digital
    // P0.2  -  Unassigned,  Open-Drain, Digital
    // P0.3  -  Unassigned,  Open-Drain, Digital
    // P0.4  -  Unassigned,  Open-Drain, Digital
    // P0.5  -  Unassigned,  Push-Pull,  Digital
    // P0.6  -  Skipped,     Open-Drain, Analog
    // P0.7  -  Skipped,     Open-Drain, Analog

    // P1.0  -  Unassigned,  Open-Drain, Digital
    // P1.1  -  Unassigned,  Open-Drain, Digital
    // P1.2  -  Unassigned,  Open-Drain, Digital
    // P1.3  -  Unassigned,  Open-Drain, Digital
    // P1.4  -  Unassigned,  Open-Drain, Digital
    // P1.5  -  Unassigned,  Open-Drain, Digital
    // P1.6  -  Unassigned,  Open-Drain, Digital
    // P1.7  -  Unassigned,  Open-Drain, Digital

    // P2.0  -  Unassigned,  Open-Drain, Digital
    // P2.1  -  Unassigned,  Open-Drain, Digital
    // P2.2  -  Unassigned,  Push-Pull,  Digital
    // P2.3  -  Unassigned,  Open-Drain, Digital
    // P2.4  -  Unassigned,  Open-Drain, Digital
    // P2.5  -  Unassigned,  Open-Drain, Digital
    // P2.6  -  Unassigned,  Open-Drain, Digital
    // P2.7  -  Unassigned,  Open-Drain, Digital

    // P3.0  -  Unassigned,  Open-Drain, Digital
    // P3.1  -  Unassigned,  Open-Drain, Digital
    // P3.2  -  Unassigned,  Open-Drain, Digital
    // P3.3  -  Unassigned,  Open-Drain, Digital
    // P3.4  -  Unassigned,  Open-Drain, Digital
    // P3.5  -  Unassigned,  Open-Drain, Digital
    // P3.6  -  Unassigned,  Open-Drain, Digital
    // P3.7  -  Unassigned,  Open-Drain, Digital

    P0MDIN    = 0x3F;
    P0MDOUT   = 0x23;
    P2MDOUT   = 0x04;
    P0SKIP    = 0xC0;
    XBR1      = 0x40;
}




void Interrupts_Init()
{
    IT01CF    = 0xBA;
    IE        = 0x87;
}

// Initialization function for device,
// Call Init_Device() from your main program
void Init_Device()
{
    PCA_Init();
    Timer_Init();
    Port_IO_Init();
    Oscillator_Init();
    Interrupts_Init();
}

void set_speed(){
	
	PUMP_ON		= 1;
	CONTROL_ON	= 1;
	
	while(speed_ticks_last_interval != speed_memory){
		 _nop_();
	}
	
	PUMP_ON			= 0;
	speed_is_set	= 1;
}



static void check_set(){
	
	if(SPEED_UP){
		if(!speed_is_set){
			speed_memory = speed_ticks_last_interval;
			set_speed();
		}   
	    else{
			PUMP_ON		= 1;
			CONTROL_ON	= 1;
			
			while(SPEED_UP){
				_nop_();
			}
			idle();
			speed_memory = speed_ticks_last_interval;
		}        
	}
}


static void check_resume(){
	if(SPEED_RESUME && speed_is_set && speed_memory){
        set_speed();
    }
}

static void check_velocity(){

	float curr_velocity = speed_ticks_last_interval * ROTATIONS_PER_MILE_PER_HOUR_PER_CLOCK_INTERVAL;

	if((curr_velocity + VELOCITY_TOLERENCE) < speed_memory){
		
		PUMP_ON		= 1;
		CONTROL_ON	= 1;
		
		while(speed_ticks_last_interval != speed_memory){
			 _nop_();
		}
		
		PUMP_ON		= 0;	

	}
	else if((curr_velocity - VELOCITY_TOLERENCE > speed_memory)){
		PUMP_ON		= 0;
		CONTROL_ON	= 0;

		while(speed_ticks_last_interval != speed_memory){
			 _nop_();
		}

		CONTROL_ON	= 1;
	}
		

}


int main(){

	Init_Device();
	PUMP_ON = 0;

	while(1){
		STATUS_LED = ~STATUS_LED;
		if(speed_is_set){
			check_velocity();
		}
		check_resume();
		check_set();
		idle();
	}
}


void TIMER0_ISR() interrupt INTERRUPT_TIMER0 {
   	
	TH0       = 0x0B;
    TL0       = 0xBD;
    TF0       = 0x00;

	speed_ticks_last_interval = speed_ticks_curr;
	speed_ticks_curr = 0;

    EX1 = 1;
}

void SPEED_SENSOR_ISR() interrupt INTERRUPT_INT0 {
 	  speed_ticks_curr++;
}

void BRAKE_CLUTCH() interrupt INTERRUPT_INT1{
   	  
	  PUMP_ON 		= 0;
	  CONTROL_ON 	= 0;
	  speed_is_set	= 0;
	  EX1 = 0;
	    
}


static void idle(){
	static unsigned char x = 0;
	PCON = PCON | 0x01;
	x++;
	x++;
	x++;
}
